#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <cvctracker/cvc_tracker.h>
#include <iostream>
#include <ctype.h>

using namespace cv;
using namespace std;

Point2f pt;
bool addRemovePt = false;
enum detecFeature{CHESSBOARD, NATURAL};
bool isNaturalAvalible = false;
int detecFeatureMode = CHESSBOARD;
bool enhancedMatching = false;
bool needToInit = true;

Mat img_matches;

void printmat(cv::Mat M)
{
	for(int i = 0; i < M.rows; i++)
	{
		const double* Mi = M.ptr<double>(i);
		for(int j = 0; j < M.cols; j++)
			printf(" %lf ",Mi[j]);
		printf("\n");
	}
	printf("\n");printf("\n");
}


void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ )
{
    if( event == CV_EVENT_LBUTTONDOWN )
    {
        pt = Point2f((float)x,(float)y);
        addRemovePt = true;
    }
}

int main( int argc, char** argv )
{
    VideoCapture cap;
    TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
    Size subPixWinSize(10,10), winSize(31,31);
	vector<Point2f> points;
    
    const int MAX_COUNT = 500;
    bool nightMode = false;
    
    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        cap.open(argc == 2 ? argv[1][0] - '0' : 0);
    else if( argc == 2 )
        cap.open(argv[1]);

    if( !cap.isOpened() )
    {
        cout << "Could not initialize capturing...\n";
        return 0;
    }
    
    //namedWindow( "LK Demo", 1 );
    //setMouseCallback( "LK Demo", onMouse, 0 );

	 Mat gray, prevGray, image;

	 cv::SurfFeatureDetector detector(600);
	 cv::SurfDescriptorExtractor extractor;
	 cv::DescriptorMatcher *matcher  = new cv::BFMatcher(cv::NORM_L2,true);
	 CVCTracker cvct(&detector , &extractor , matcher);

#if 1
	 isNaturalAvalible = cvct.loadMarkerImage("d:\\android16\\temp\\obidosRR.png");
#else
	 cv::Mat imageFile = cv::imread("d:\\android16\\temp\\obidosRR.png");
	 if (imageFile.data == NULL)
	 {
		isNaturalAvalible =  false;
	 }else
		cvct.loadMarkerImage(imageFile);
#endif

	 for(;;)
	 {
		 Mat frame;
		 Mat glProj, glMv;
		 cap >> frame;
		 if (frame.empty())
			 break;
		 //if(0)
			//frame.copyTo(image);
		 //else
			 cvtColor(frame, image, CV_BGR2RGB);
		 cvtColor(frame, gray, CV_BGR2GRAY); 
#if 0
		 if (detecFeatureMode == CHESSBOARD)
		 {
			 if (needToInit)
			 {
				 if (cvct.matchChessboard(gray))
				 {
					 cvct.initObjectPoints3D();
				     cvct.calibrationCamera();
					 cvct.drawReprojection(image);
					 needToInit = false;
				 }
			 }
			 else
			 {
				 cvct.lkt( gray, prevGray, points );
				 if (cvct.calibrationCamera())
					 cvct.drawReprojection(image);
				 else
					 needToInit = true;
			 }

			 imshow("ChessBoard-natural Demo", image);
		 }
		 else if (detecFeatureMode == NATURAL)
		 {
			 if (needToInit)
			 {
				 cvct.match(gray);
				 cvct.calibrationCamera();
				 cvct.drawReprojection(image);
				 cvct.lktSurf();
				 needToInit = false;
			 }
			 else
			 {
				 cvct.lkt( gray, prevGray, points );
				 if (cvct.calibrationCamera())
					 cvct.drawReprojection(image);
				 else
					 needToInit = true;
			 }

			 imshow("ChessBoard-natural Demo", image);
		 }
		 else
			 return 1;

		 swap( prevGray, gray );
		 cvct.swapPoints( points );
#else
		 cvct.faztudo(prevGray,gray);
		 cvct.drawReprojection(image);
		 swap( prevGray, gray );
		  cvtColor(image, image, CV_BGR2RGB);
		 imshow("ChessBoard-natural Demo", image);
#endif
		 char c = (char)waitKey(1);
		 if (c == 27)
			 break;
		 switch (c)
		 {
		 case 'r':
			 needToInit = true;
			 break;
		 //case 'c':
			 //points[1].clear();
			 //break;
			 /*case 'n':
			 nightMode = !nightMode;
			 break;*/
		 case 'k' :
			 cvct.useKLT(true);
			 break;
		 case 'j' :
			 cvct.useKLT(false);
			 break;
		 case 'd':
			 detecFeatureMode = CHESSBOARD;
			 break;
		 case 'n':
			 detecFeatureMode = NATURAL;
			 break;
		 case 'e':
			 enhancedMatching = (enhancedMatching)?false:true;
			 break;
		 default:
			 ;
		 }
	 }
}
